/**
 * @brief scheduling stuff implementation
 * @author Gilad Avidov
 *
 */
#include <mcs51reg.h>
#include <stdio.h>
#include <setjmp.h>
#include "sched.h"
#include "timer.h"

#define USEC_CNT   (0x01)
#define SCHED_OFST_TO_RET_ADDR_HI   (14)
#define SCHED_OFST_TO_RET_ADDR_LOW  (SCHED_OFST_TO_RET_ADDR_HI + 1)

typedef __data unsigned char * __data sched_stack_ptr_t;

typedef enum sched_state {
    SCHED_STATE_NONE,   // uninitializes
    SCHED_STATE_READY,
    SCHED_STATE_BLOKED,
    SCHED_STATE_CNT,
} sched_state_t ;

/* task control block */
typedef struct {
    unsigned char       stack[STACK_SIZE];
    sched_state_t       state;
    unsigned short      sleep_ticks; // when non zero task is suspended
} sched_task_t;

/* order implies priority */
xdata sched_task_t sched_tasks[SCHED_MAX_TASSKS];

volatile unsigned char current_task_id = 0;
//volatile unsigned char current_stack_ptr = 0;
const unsigned char sched_stack_size = STACK_SIZE;

unsigned char cur_stack_low;
unsigned char cur_stack_high;
unsigned char nxt_stack_low;
unsigned char nxt_stack_high;
unsigned char sched_fork_task_high;
unsigned char sched_fork_task_low;

unsigned char sched_get_highest_priority_ready_task();
void sched_context_switch(unsigned char next_task_id) __critical;

void sched_sleep_ticks(unsigned short ticks)
{
    unsigned char next_task_id;

    //printf("msleep cur:%d\n\r", current_task_id);
    sched_tasks[current_task_id].sleep_ticks = ticks;

    while (sched_tasks[current_task_id].sleep_ticks) {
        next_task_id = sched_get_highest_priority_ready_task();
        //printf("msleep cur:%d next:%d going to switch\n\r", current_task_id, next_task_id);
        //putchar('.'); // idle

        if (next_task_id < SCHED_MAX_TASSKS)
            sched_context_switch(next_task_id);
    }
}

void sched_tick()
{
    unsigned char i;
    unsigned char next_task_id = current_task_id;

    for (i=0 ; i<SCHED_MAX_TASSKS ; ++i)
        if (sched_tasks[i].sleep_ticks)
            --sched_tasks[i].sleep_ticks;

    /*next_task_id = sched_get_highest_priority_ready_task();

    if ((next_task_id != current_task_id) && (next_task_id < SCHED_MAX_TASSKS))
        sched_context_switch(next_task_id);*/
}

unsigned char sched_get_highest_priority_ready_task()
{
    unsigned char i;
    unsigned char next_task_id = SCHED_MAX_TASSKS;

    for (i=0 ; i<SCHED_MAX_TASSKS ; ++i) {
        //printf("get prio cur:%u task[%u].state:%u\n\r",
        //       current_task_id, i, sched_tasks[i].state);
        if (!sched_tasks[i].sleep_ticks) {
            next_task_id = i;
            break;
        }
    }
    //printf("sched_get_highest_priority_ready_task():%d\n\r", next_task_id);
    return next_task_id;
}

/*
 * save context to the global
 * cur_stack_low and cur_stack_high
*/
void sched_save_context() _naked
{
__asm
    ;push all the registers
    push	acc
    push	b
    push	dpl
    push	dph
    push	(0+2)
    push	(0+3)
    push	(0+4)
    push	(0+5)
    push	(0+6)
    push	(0+7)
    push	(0+0)
    push	(0+1)
    push	psw
    mov	psw,#0x00

    ; set dptr to point to the external stack
    mov	dptr,#_cur_stack_low
    movx a,@dptr
    mov	r2,a

    mov	dptr,#_cur_stack_high
    movx a,@dptr
    mov	r3,a

    mov dpl,r2
    mov dph,r3

    ; copy loop inspierd by http://www.bradrodriguez.com/papers/8051task.pdf
    ; copy from from stack pointer to memory begining
    mov a,sp
    movx @dptr,a           ; first thing in current->stack is the sp==stack used
    inc dptr
    mov r0,a               ; r0 gets the stack pointer

save_stack:
    mov a,@r0               ; a gets sontent of the stack
    movx @dptr,a            ; copy a to extranl mem
    inc dptr                ; inc extrenal memory ptr
    djnz r0,save_stack      ; until we copy the stack all the way down

    pop	psw
    pop	(0+1)
    pop	(0+0)
    pop	(0+7)
    pop	(0+6)
    pop	(0+5)
    pop	(0+4)
    pop	(0+3)
    pop	(0+2)
    pop	dph
    pop	dpl
    pop	b
    pop	acc

    ret
__endasm;
//return 0;
}

/*
 * save context to the global
 * nxt_stack_low and nxt_stack_high
*/
void sched_restore_context() _naked
{
__asm
    ; set dptr to point to the external stack of the next task
    mov	dptr,#_nxt_stack_low
    movx a,@dptr
    mov	r2,a

    mov	dptr,#_nxt_stack_high
    movx a,@dptr
    mov	r3,a

    mov dpl,r2
    mov dph,r3

    ; read the stack pointer which was the first to be stored
    ; copy loop inspierd by http://www.bradrodriguez.com/papers/8051task.pdf
    movx a,@dptr
    inc dptr
    mov sp,a                ; restore stack pointer
    mov r0,a                ; r0 has the new sp

restore_stack:
    movx    a,@dptr
    inc     dptr
    mov     @r0,a
    djnz    r0,restore_stack

    pop	psw
    pop	(0+1)
    pop	(0+0)
    pop	(0+7)
    pop	(0+6)
    pop	(0+5)
    pop	(0+4)
    pop	(0+3)
    pop	(0+2)
    pop	dph
    pop	dpl
    pop	b
    pop	acc

    ret
__endasm;
}

void sched_context_switch_asm(void) _naked
{
__asm
    ;push all the registers
    push	acc
    push	b
    push	dpl
    push	dph
    push	(0+2)
    push	(0+3)
    push	(0+4)
    push	(0+5)
    push	(0+6)
    push	(0+7)
    push	(0+0)
    push	(0+1)
    push	psw
    mov	psw,#0x00

    ; set dptr to point to the external stack of the current task
    mov	dptr,#_cur_stack_low
    movx a,@dptr
    mov	r2,a

    mov	dptr,#_cur_stack_high
    movx a,@dptr
    mov	r3,a

    mov dpl,r2
    mov dph,r3

    ; copy loop inspierd by http://www.bradrodriguez.com/papers/8051task.pdf
    ; copy from from stack pointer to memory begining
    mov a,sp
    movx @dptr,a           ; first thing in current->stack is the sp==stack used
    inc dptr
    mov r0,a               ; r0 gets the stack pointer

switch_save_stack:
    mov a,@r0               ; a gets sontent of the stack
    movx @dptr,a            ; copy a to extranl mem
    inc dptr                ; inc extrenal memory ptr
    djnz r0,switch_save_stack  ; until we copy the stack all the way down

    ;;;;;;;; restore ;;;;;;;;

    ; set dptr to point to the external stack of the next task
    mov	dptr,#_nxt_stack_low
    movx a,@dptr
    mov	r2,a

    mov	dptr,#_nxt_stack_high
    movx a,@dptr
    mov	r3,a

    mov dpl,r2
    mov dph,r3

    ; read the stack pointer which was the first to be stored
    ; copy loop inspierd by http://www.bradrodriguez.com/papers/8051task.pdf
    movx a,@dptr
    inc dptr
    mov sp,a                ; restore stack pointer
    mov r0,a                ; r0 has the new sp

switch_restore_stack:
    movx    a,@dptr
    inc     dptr
    mov     @r0,a
    djnz    r0,switch_restore_stack

    pop	psw
    pop	(0+1)
    pop	(0+0)
    pop	(0+7)
    pop	(0+6)
    pop	(0+5)
    pop	(0+4)
    pop	(0+3)
    pop	(0+2)
    pop	dph
    pop	dpl
    pop	b
    pop	acc

    ret
__endasm;
}

#define SCHED_MEM_DUMP_LINE_BC  (10)
void sched_dump_external_mem(unsigned char * __xdata mem, unsigned char len)
{
    unsigned char bc;

    puts("dump of thread stack area in external RAM:\n\r");

    for (bc = 0 ; len ; --len, ++bc) {
        if (!(bc % SCHED_MEM_DUMP_LINE_BC))
            printf("\n\r%04p: ", mem + bc);
        printf("0x%02x ", mem[bc]);
    }
    puts("\n\r");
}

void sched_dump_internal_mem(unsigned char len)
{
    unsigned char bc;
    unsigned char * __data mem = 0;

    puts("dump of stack area in internal RAM:\n\r");

    for (bc = 0 ; len ; --len, ++bc, ++mem) {
        if (!(bc % SCHED_MEM_DUMP_LINE_BC))
            printf("\n\r%04p: ", mem);
        printf("0x%02x ", *mem);
    }
    puts("\n\r");
}


void sched_context_switch(unsigned char next_task_id) __critical
{
    unsigned short tmp;
    unsigned char * __xdata external   = &sched_tasks[current_task_id].stack;

    external = &sched_tasks[current_task_id].stack;
    tmp = ((unsigned short) external);
    cur_stack_low  = tmp & 0xff;
    cur_stack_high = (tmp >> 8) & 0xff;

    current_task_id = next_task_id;

    external = &sched_tasks[current_task_id].stack;
    tmp = ((unsigned short) external);
    nxt_stack_low  = tmp & 0xff;
    nxt_stack_high = (tmp >> 8) & 0xff;

    sched_context_switch_asm();
}



void usleep(unsigned short usec)
{
    unsigned short i;

    for (; usec ; --usec)
            for (i = 0; i < USEC_CNT; ++i);
}

char sched_fork(void (*task)(void))
{
    char i;
    unsigned char len;
    unsigned short tmp;
    unsigned char * __xdata external;
    char * __xdata task_ptr = task;

    // find available task slot
    for (i = 0 ; i < SCHED_MAX_TASSKS ; ++i) {
        if (sched_tasks[i].state == SCHED_STATE_NONE)
            break;
    }

    if (i == SCHED_MAX_TASSKS) {
        puts("error: cannot create any more tasks\n\r.");
        return -1;
    }

    sched_tasks[i].state = SCHED_STATE_READY;

    external   = sched_tasks[i].stack;
    tmp = ((unsigned short) external);
    cur_stack_low  = tmp & 0xff;
    cur_stack_high = (tmp >> 8) & 0xff;

    tmp = ((unsigned short) task_ptr);
    sched_fork_task_low  = tmp & 0xff;
    sched_fork_task_high = (tmp >> 8) & 0xff;

    sched_save_context();

    len = external[0]; // len = SP
    printf("fork() copy context to:0x%02x%02x SP:0x%x orig-PC:0x%02x%02x new-PC:0x%02x%02x\n\r",
            cur_stack_high, cur_stack_low, len,
            external[SCHED_OFST_TO_RET_ADDR_HI],
            external[SCHED_OFST_TO_RET_ADDR_LOW],
            sched_fork_task_high, sched_fork_task_low);

    external[SCHED_OFST_TO_RET_ADDR_HI]  = sched_fork_task_high;
    external[SCHED_OFST_TO_RET_ADDR_LOW] = sched_fork_task_low;

    return i;
}

void sched_run()
{
    unsigned char * __xdata external   = &sched_tasks[current_task_id].stack;
    unsigned short tmp;

    external = &sched_tasks[current_task_id].stack;
    tmp = ((unsigned short) external);
    nxt_stack_low  = tmp & 0xff;
    nxt_stack_high = (tmp >> 8) & 0xff;

    timer0_run(); // calls the wakeup
    sched_restore_context();
}

void sched_init()
{
    unsigned char i;
    for (i = 0 ; i < SCHED_STATE_CNT ; ++i){
        sched_tasks[i].state = SCHED_STATE_NONE;
    }
    current_task_id =0;
}

void sched_dump_full_state(void)
{
    unsigned char i;
    unsigned char * __xdata stack;
    unsigned short tmp;
    unsigned char len;
    unsigned char addr_hi, addr_low;

    sched_dump_state();

    printf("current task[%u]\n\r", current_task_id);
    stack    = &sched_tasks[current_task_id].stack;
    len      = stack[0];
    sched_dump_internal_mem(len);

    for (i=0 ; i<SCHED_MAX_TASSKS ; ++i) {
        stack    = &sched_tasks[i].stack;
        tmp      = ((unsigned short) stack);
        len      = stack[0];
        addr_hi  = stack[SCHED_OFST_TO_RET_ADDR_HI];
        addr_low = stack[SCHED_OFST_TO_RET_ADDR_LOW];

        printf("task[%u] SP:0x%x PC:0x%02x%02x\n\r",i, len, addr_hi, addr_low);
        sched_dump_external_mem(stack, len);
    }
}

void sched_dump_state(void)
{
    unsigned char i;
    unsigned char * __xdata stack;
    unsigned short tmp;
    unsigned char len;

    printf("current task[%u]\n\r", current_task_id);

    for (i=0 ; i<SCHED_MAX_TASSKS ; ++i) {
        stack    = &sched_tasks[i].stack;
        tmp      = ((unsigned short) stack);
        len      = stack[0];

        if (sched_tasks[i].sleep_ticks)
            printf("task[%u] sleep stack-size:%03u ptr:0x%04x wakeup:%02u\n\r",
                   i, len, tmp, sched_tasks[i].sleep_ticks);
        else
            printf("task[%u] ready stack-size:%03u ptr:0x%04x\n\r",
                   i, len, tmp, sched_tasks[i].sleep_ticks);
    }
}
